#if defined(MODBUS_TCP)|| defined(MODBUS_RTU)
/*
 * Copyright (c) 2001-2003,2016 Mario de Sousa (msousa@fe.up.pt)
 *
 * This file is part of the Modbus library for Beremiz and matiec.
 *
 * This Modbus library is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser
 * General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this Modbus library.  If not, see <http://www.gnu.org/licenses/>.
 *
 * This code is made available on the understanding that it will not be
 * used in safety-critical situations without a full and competent review.
 */

/* mb_master.c */

#include <fcntl.h> /* File control definitions */
#include <cstdio> /* Standard input/output */

#ifdef ARDUINO
#elif defined(windows_x86)

#include <windows.h>

#endif
#ifdef __linux__
#include <termio.h>     /* POSIX terminal control definitions */
#include <netinet/in.h> /* required for htons() and ntohs() */
#endif

#include <ctime> /* Time structures for select() */
#include <logger_rte.h>
#include <plc_modbus.h>

#include "mb_layer1.h"
#include "mb_master.h"
#include "mb_master_private.h"
#include"Rtos.h"

/* #define DEBUG */ /* uncomment to see the data sent and received */

#define modbus_write fptr_[layer1_fin].modbus_write
#define modbus_read fptr_[layer1_fin].modbus_read
#define modbus_init fptr_[layer1_fin].modbus_init
#define modbus_done fptr_[layer1_fin].modbus_done
#define modbus_connect fptr_[layer1_fin].modbus_connect
#define modbus_listen fptr_[layer1_fin].modbus_listen
#define modbus_close fptr_[layer1_fin].modbus_close
#define modbus_silence_init fptr_[layer1_fin].modbus_silence_init
#define modbus_get_min_timeout fptr_[layer1_fin].modbus_get_min_timeout

/* the lower two bits of ttyfd are used to store the index to layer1 function pointers */
/* layer1_fin index to fptr_[] is in lowest 2 bits of fd */
#define get_ttyfd()        \
  int layer1_fin = node->node_address.naf; \
  int ttyfd = node->fd ;

/******************************************/
/******************************************/
/**                                      **/
/**         Global Variables...          **/
/**                                      **/
/******************************************/
/******************************************/
/* The layer 1 (RTU, ASCII, TCP) implementation will be adding some headers and CRC (at the end)
    *  of the packet we build here (actually currently it is only at the end). Since we want to
    *  re-use the same buffer so as not to continuosly copy the same info from buffer to buffer,
    *  we need tp allocate more bytes than the ones we need for this layer. Therefore, the
    *  extra_bytes parameter.
    *
    *  Note that we add one more extra byte. This is because some packets will not be
    *  starting off at byte 0, but rather at byte 1 of the buffer. This is in order to guarantee
    *  that the data that is sent on the buffer is aligned on even bytes (the 16 bit words!).
    *  This will allow us to reference this memory as an u16 *, without producing 'bus error'
    *  messages in some embedded devices that do not allow acessing u16 on odd numbered addresses.
    */
static int buff_extra_bytes_;
#define QUERY_BUFFER_SIZE (MAX_L2_FRAME_LENGTH + buff_extra_bytes_ + 1)

/******************************************/
/******************************************/
/**                                      **/
/**       Local Utility functions...     **/
/**                                      **/
/******************************************/
/******************************************/

/*
 * Function to determine next transaction id.
 *
 * We use a library wide transaction id, which means that we
 * use a new transaction id no matter what slave to which we will
 * be sending the request...
 */
static inline u16 next_transaction_id() {
    static u16 next_id = 0;
    return next_id++;
}

/*
 * Functions to convert u16 variables
 * between network and host byte order
 *
 * NOTE: Modbus uses MSByte first, just like
 *       tcp/ip, so we use the htons() and
 *       ntoh() functions to guarantee
 *       code portability.
 */
static inline u16 mb_hton(u16 h_value) {
    return (u16) ((h_value << 8) | (h_value >> 8));
    // #if !defined(USE_LWIP)
    //   return htons(h_value);
    // #else
    //   return lwip_htons(h_value);
    // #endif
}

static inline u16 mb_ntoh(u16 m_value) {

    // #if !defined(USE_LWIP)
    return (u16) ((m_value << 8) | (m_value >> 8));
    // return ntohs(m_value);
    // #else
    //   return lwip_ntohs(m_value);
    // #endif
}

static inline u8 msb(u16 value) { return (value >> 8) & 0xFF; }

static inline u8 lsb(u16 value) { return value & 0xFF; }

/*************************************************/
/*************************************************/
/**                                             **/
/**   Common functions for Modbus Protocol.     **/
/**                                             **/
/*************************************************/
/*************************************************/

/* build the common elements of a query frame */
static inline int build_packet(u8 slave,
                               u8 function,
                               u16 start_addr,
                               u16 count,
                               u8 *packet) {
    union {
        u16 u_16;
        u8 u_8[2];
    } tmp{};

    packet[0] = slave;
    packet[1] = function;
    /* NOTE:
       *  Modbus uses high level addressing starting off from 1, but
       *  this is sent as 0 on the wire!
       *  We could expect the user to specify high level addressing
       *   starting at 1, and do the conversion to start off at 0 here.
       *   However, to do this we would then need to use an uint32_t data type
       *   to correctly hold the address supplied by the user (which could
       *   correctly be 65536, which does not fit in an u16), which would
       *   in turn require us to check whether the address supplied by the user
       *   is correct (i.e. <= 65536).
       *  I decided to go with the other option of using an u16, and
       *   requiring the user to use addressing starting off at 0!
       */
    /* NOTE: we do not use up casting - i.e. the following
     *       *((u16 *)(packet+2)) = mb_hton(start_addr);
     *       because packet+2 is NOT aligned with an even address, and would
     *       therefore result in 'bus error' when using compilers that do not
     *       automatically do the required decomposing of this supposedly
     *       single bus access into two distinct bus accesses.
     *       (Note that some compilers do do this decomposing automatically
     *       in which case the following is not necessary).
     *       At the moment, I (Mario de Sousa) know of at least one cross-compiler
     *       that does not do the decomposing automatically, i.e. the
     *       AVR32 cross-compiler.
     */
    tmp.u_16 = mb_hton(start_addr);
    packet[2] = tmp.u_8[0];
    packet[3] = tmp.u_8[1];
    tmp.u_16 = mb_hton(count);
    packet[4] = tmp.u_8[0];
    packet[5] = tmp.u_8[1];

    return 6;
}

/* Execute a Query/Response transaction between client and server */
/* returns: <0    -> ERROR: error codes
 *          >2    -> SUCCESS: frame length
 *           0..2 -> will never be returned!
 */
static int mb_transaction(u8 *packet,
                          int query_length,
                          u8 **data,
                          int send_retries,
                          u8 *error_code,
                          uint32_t response_timeout, u8 crc_reverse, client_node_t *node) {
    int error = MODBUS_TIMEOUT;
    int response_length = INTERNAL_ERROR;
    u16 send_transaction_id;
    u16 recv_transaction_id;
    int layer1_fin = node->node_address.naf; \
    int ttyfd = node->fd;
    /* We must also initialize the recv_transaction_id with the same value,
       * since some layer 1 protocols do not support transaction id's, so
       * simply return the recv_transaction_id variable without any changes...
       */
    /* NOTE: we re-use the same transaction id for all send re-tries., since, in truth,
     * it is still the same transaction. This will also simplify re-synchronising with
     * some slaves that keep a buffer of outstanding requests, and will reply to all of
     * them, in FIFO order. In this case, once an error occurs we will be swamping the
     * slave with requests. By using the same transaction id, we may correctly consider
     * the reply to the first request sent as the reply to the third request! This means
     * we stop re-trying the sending of further requests, and no longer swamp the slave...
     */
    send_transaction_id = recv_transaction_id = next_transaction_id();

    for (send_retries++; send_retries > 0; send_retries--) {
        error = MODBUS_TIMEOUT;

        if (modbus_write(node->nd, packet, (u16) query_length, send_transaction_id, response_timeout, crc_reverse) < 0) {
            error = PORT_FAILURE;
            continue;
        }

        /* if we receive a correct response but with a wrong transaction id or wrong modbus function, we try to
           * receive another frame instead of returning an error or re-sending the request! This first frame could
           * have been a response to a previous request of ours that timed out waiting for a response, and the
           * response we are waiting for could be coming 'any minute now'.
           */
        do {
            response_length = modbus_read(&node->nd, data, &recv_transaction_id,
                                          packet, (u16) query_length, response_timeout, crc_reverse);

            /* TIMEOUT condition */
            /* However, if we had previously received an invalid frame, or some other error,
             * we return that error instead!
             * Note that the 'error' variable was initialised with the TIMEOUT error
             * condition, so if no previous error ocurred, we will be returning the
             * TIMEOUT error condition here!
             */
            if (response_length == -2)
                return error;
            /* NOTE we want to break out of this while loop without even running the while()
             * condition, as that condition is only valid if response_length > 3 !!
             */
            if (response_length < 0) {
                error = PORT_FAILURE;
                break;
            }
            /* This should never occur! Modbus_read() should only return valid frames! */
            if (response_length < 3)
                return INTERNAL_ERROR;

        } while (/* we have the wrong transaction id */
                (send_transaction_id != recv_transaction_id)
                /* not a response frame to _our_ query */
                ||
                (((*data)[1] & ~0x80) != packet[1]) || ((*data)[0] != packet[0])
            /* NOTE: no need to check whether (*data)[0] = slave!              */
            /*       This has already been done by the modbus_read() function! */
                );

        if (response_length < 0) {
            error = PORT_FAILURE;
            continue;
        }

        /* Now check whether we received a Modbus Exception frame */
        if ((((*data)[1] & 0x80) != 0)) { /* we have an exception frame! */
            /* NOTE: we have already checked above that data[2] exists! */
            if (error_code != nullptr)
                *error_code = (*data)[2];
            return MODBUS_ERROR;
        }
        /* success! Let's get out of the send retry loop... */
        return response_length;
    }
    /* reached the end of the retries... */
    return error;
}

/**************************************/
/**************************************/
/**                                  **/
/**   Modbus Protocol Functions.     **/
/**                                  **/
/**************************************/
/**************************************/

/* Execute a transaction for functions that READ BITS.
 * Bits are stored on an int array, one bit per int.
 * Called by:  read_input_bits()
 *             read_output_bits()
 */
static int read_bits(u8 function,
                     u8 slave,
                     u16 start_addr,
                     u16 count,
                     u16 *dest,
                     int dest_size,
                     int send_retries,
                     u8 *error_code,
                     uint32_t response_timeout,
                     void *data_access_mutex,
                     u8 crc_reverse, client_node_t *node
) {

    u8 packet[QUERY_BUFFER_SIZE];
    u8 *data;
    int query_length;
    int response_length;
    int temp;
    int dest_pos = 0;
    int coils_processed = 0;

    query_length = build_packet(slave, function, start_addr, count, packet);
    if (query_length < 0)
        return INTERNAL_ERROR;

    response_length = mb_transaction(packet, query_length, &data,
                                     send_retries, error_code, response_timeout, crc_reverse, node);

    if (response_length < 0)
        return response_length;
    /* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */
    if (response_length != 3 + (count + 7) / 8)
        return INVALID_FRAME;
    if (data[2] != (count + 7) / 8)
        return INVALID_FRAME;

    if (nullptr != data_access_mutex)
        Rtos::mutex_lock(data_access_mutex);
    for (int i = 0; (i < data[2]) && (i < dest_size); i++) {
        temp = data[3 + i];
        for (int bit = 0x01; (bit & 0xff) && (coils_processed < count);) {
            dest[dest_pos] = (temp & bit) ? 1 : 0;
            coils_processed++;
            dest_pos++;
            bit = bit << 1;
        }
    }
    if (nullptr != data_access_mutex)
        Rtos::mutex_unlock(data_access_mutex);
    return response_length;
}

/* Execute a transaction for functions that READ BITS.
 * Bits are stored on an uint32_t array, 32 bits per uint32_t.
 * Unused bits in last uint32_t are set to 0.
 * Called by:  read_input_bits_u32()
 *             read_output_bits_u32()
 */
static int read_bits_u32(u8 function,
                         u8 slave,
                         u16 start_addr,
                         u16 count, /* number of bits !! */
                         uint32_t *dest,
                         int send_retries,
                         u8 *error_code,
                         uint32_t response_timeout,
                         u8 crc_reverse, client_node_t *node) {
    u8 packet[QUERY_BUFFER_SIZE];
    u8 *data;
    int response_length;
    int query_length;
    int dest_pos;
    int byte_count;
    int i;

    query_length = build_packet(slave, function, start_addr, count, packet);
    if (query_length < 0)
        return INTERNAL_ERROR;

    response_length = mb_transaction(packet, query_length, &data, send_retries, error_code, response_timeout,
                                     crc_reverse, node);

    if (response_length < 0)
        return response_length;
    /* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */
    if (response_length != 3 + (count + 7) / 8)
        return INVALID_FRAME;
    if (data[2] != (count + 7) / 8)
        return INVALID_FRAME;

    byte_count = data[2];
    data += 3;
    /* handle groups of 4 bytes... */
    for (i = 0, dest_pos = 0; i + 3 < byte_count; i += 4, dest_pos++)
        dest[dest_pos] = data[i] + data[i + 1] * 0x100 + data[i + 2] * 0x10000 + data[i + 3] * 0x1000000;
    /* handle any remaining bytes... begining with the last! */
    if (i < byte_count)
        dest[dest_pos] = 0;
    for (byte_count--; i <= byte_count; byte_count--)
        dest[dest_pos] = dest[dest_pos] * 0x100 + data[byte_count];

    return response_length;
}

/* FUNCTION 0x01   - Read Coils
 * Bits are stored on an int array, one bit per int.
 */
int read_output_bits(u8 slave,
                     u16 start_addr,
                     u16 count,
                     u16 *dest,
                     int dest_size,
                     int send_retries,
                     u8 *error_code,
                     uint32_t response_timeout,
                     void *data_access_mutex,
                     u8 crc_reverse, client_node_t *node
) {
    if (count > MAX_READ_BITS) {
        count = MAX_READ_BITS;
#ifdef DEBUG
        logger.debug( "Too many coils requested.\n");
#endif
    }

    return read_bits(0x01 /* function */,
                     slave, start_addr, count, dest, dest_size, send_retries, error_code, response_timeout,
                     data_access_mutex, crc_reverse, node);
}

/* FUNCTION 0x01   - Read Coils
 * Bits are stored on an uint32_t array, 32 bits per uint32_t.
 * Unused bits in last uint32_t are set to 0.
 */
int read_output_bits_u32(u8 slave,
                         u16 start_addr,
                         u16 count,
                         uint32_t *dest,
                         int ttyfd,
                         int send_retries,
                         u8 *error_code,
                         uint32_t response_timeout,
                         u8 crc_reverse, client_node_t *node) {
    if (count > MAX_READ_BITS) {
        count = MAX_READ_BITS;
#ifdef DEBUG
        logger.debug( "Too many coils requested.\n");
#endif
    }

    return read_bits_u32(0x01 /* function */,
                         slave, start_addr, count, dest, send_retries, error_code, response_timeout, crc_reverse, node);
}

/* FUNCTION 0x02   - Read Discrete Inputs
 * Bits are stored on an int array, one bit per int.
 */
int read_input_bits(u8 slave,
                    u16 start_addr,
                    u16 count,
                    u16 *dest,
                    int dest_size,
                    int send_retries,
                    u8 *error_code,
                    uint32_t response_timeout,
                    void *data_access_mutex,
                    u8 crc_reverse, client_node_t *node
) {
    if (count > MAX_READ_BITS) {
        count = MAX_READ_BITS;
#ifdef DEBUG
        logger.debug( "Too many coils requested.\n");
#endif
    }

    return read_bits(0x02 /* function */,
                     slave, start_addr, count, dest, dest_size, send_retries, error_code, response_timeout,
                     data_access_mutex, crc_reverse, node);
}

/* FUNCTION 0x02   - Read Discrete Inputs
 * Bits are stored on an uint32_t array, 32 bits per uint32_t.
 * Unused bits in last uint32_t are set to 0.
 */
int read_input_bits_u32(u8 slave,
                        u16 start_addr,
                        u16 count,
                        uint32_t *dest,
                        int send_retries,
                        u8 *error_code,
                        uint32_t response_timeout,
                        u8 crc_reverse, client_node_t *node) {
    if (count > MAX_READ_BITS) {
        count = MAX_READ_BITS;
#ifdef DEBUG
        logger.debug( "Too many coils requested.\n");
#endif
    }

    return read_bits_u32(0x02 /* function */,
                         slave, start_addr, count, dest, send_retries, error_code, response_timeout, crc_reverse, node);
}

/* Execute a transaction for functions that READ REGISTERS.
 * Called by:  read_input_words()
 *             read_output_words()
 */
static int read_registers(u8 function,
                          u8 slave,
                          u16 start_addr,
                          u16 count,
                          u16 *dest,
                          int dest_size,
                          int send_retries,
                          u8 *error_code,
                          uint32_t response_timeout,
                          void *data_access_mutex,
                          u8 crc_reverse, client_node_t *node
) {
    u8 *data;
    u8 packet[QUERY_BUFFER_SIZE];
    int response_length;
    int query_length;
    int temp;

    query_length = build_packet(slave, function, start_addr, count, packet);
    if (query_length < 0)
        return INTERNAL_ERROR;

    response_length = mb_transaction(packet, query_length, &data, send_retries, error_code, response_timeout,
                                     crc_reverse, node);

    if (response_length < 0)
        return response_length;
    // if (response_length != 3 + 2 * count)
    //     return INVALID_FRAME;
    if (data[2] != 2 * count)
        return INVALID_FRAME;

    if (nullptr != data_access_mutex)
        Rtos::mutex_lock(data_access_mutex, -1);
    for (int i = 0; (i < (data[2] * 2)) && (i < dest_size); i++) {
        temp = data[3 + i * 2] << 8;   /* copy reg hi byte to temp hi byte*/
        temp = temp | data[4 + i * 2]; /* copy reg lo byte to temp lo byte*/
        dest[i] = (u16) temp;
    }
    if (nullptr != data_access_mutex)
        Rtos::mutex_unlock(data_access_mutex);
    return response_length;
}

/* Execute a transaction for functions that READ REGISTERS.
 * return the array with the data to the calling function
 * Called by:  read_input_words_u16_ref()
 *             read_output_words_u16_ref()
 */

static int read_registers_u16_ref(u8 function,
                                  u8 slave,
                                  u16 start_addr,
                                  u16 count,
                                  u16 **dest,
                                  int send_retries,
                                  u8 *error_code,
                                  uint32_t response_timeout,
                                  u8 crc_reverse, client_node_t *node) {
    u8 *data;
    u8 packet[QUERY_BUFFER_SIZE];
    int response_length;
    int query_length;
    int byte_count;

    query_length = build_packet(slave, function, start_addr, count, packet);
    if (query_length < 0)
        return INTERNAL_ERROR;

    response_length = mb_transaction(packet, query_length, &data, send_retries, error_code, response_timeout,
                                     crc_reverse, node);

    if (response_length < 0)
        return response_length;
    if (response_length != 3 + 2 * count)
        return INVALID_FRAME;
    if (data[2] != 2 * count)
        return INVALID_FRAME;

    byte_count = data[2];
    data = data + 3; /* & data[3] */

    if (mb_ntoh(0x0102) != 0x0102) {
        /* little endian host... => we need to swap the bytes! */
        for (int i = 0; i < byte_count; i++) {
            /* the following 3 lines result in the two values being exchanged! */
            data[i] = data[i] ^ data[i + 1];
            data[i + 1] = data[i] ^ data[i + 1];
            data[i] = data[i] ^ data[i + 1];
        }
    }
    *dest = (u16 *) data;
    return byte_count;
}

/* Execute a transaction for functions that READ REGISTERS.
 * u16 registers are stored in array of uint32_t, two registers per uint32_t.
 * Unused bits of last uint32_t element are set to 0.
 * Called by:  read_input_words_u32()
 *             read_output_words_u32()
 */
static int read_registers_u32(u8 function,
                              u8 slave,
                              u16 start_addr,
                              u16 count,
                              uint32_t *dest,
                              int send_retries,
                              u8 *error_code,
                              uint32_t response_timeout,
                              u8 crc_reverse, client_node_t *node) {
    u8 *data;
    u8 packet[QUERY_BUFFER_SIZE];
    int response_length;
    int query_length;
    int i;
    int byte_count;
    int dest_pos;

    query_length = build_packet(slave, function, start_addr, count, packet);
    if (query_length < 0)
        return INTERNAL_ERROR;

    response_length = mb_transaction(packet, query_length, &data, send_retries, error_code, response_timeout,
                                     crc_reverse, node);

    if (response_length < 0)
        return response_length;
    if (response_length != 3 + 2 * count)
        return INVALID_FRAME;
    if (data[2] != 2 * count)
        return INVALID_FRAME;

    byte_count = data[2];
    data += 3;

    if (mb_ntoh(0x0102) == 0x0102) {
        /* big endian host... */
        /* handle groups of 4 bytes... */
        for (i = 0, dest_pos = 0; i + 3 < byte_count; i += 4, dest_pos++) {
            *(((u8 *) (dest + dest_pos)) + 0) = *(data + i + 3);
            *(((u8 *) (dest + dest_pos)) + 1) = *(data + i + 4);
            *(((u8 *) (dest + dest_pos)) + 2) = *(data + i + 0);
            *(((u8 *) (dest + dest_pos)) + 3) = *(data + i + 1);
        }
        /* handle any remaining bytes...
         * since byte_count is supposed to be multiple of 2,
         * (and has already been verified above 'if (data[2] != 2*count)')
         * this will be either 2, or none at all!
         */
        if (i + 1 < byte_count)
            *(((u8 *) (dest + dest_pos)) + 0) = 0;
        *(((u8 *) (dest + dest_pos)) + 1) = 0;
        *(((u8 *) (dest + dest_pos)) + 2) = *(data + i + 0);
        *(((u8 *) (dest + dest_pos)) + 3) = *(data + i + 1);
    } else {
        /* little endian host... */
        /* handle groups of 4 bytes... */
        for (i = 0, dest_pos = 0; i + 3 < byte_count; i += 4, dest_pos++) {
            *(((u8 *) (dest + dest_pos)) + 0) = *(data + i + 1);
            *(((u8 *) (dest + dest_pos)) + 1) = *(data + i + 0);
            *(((u8 *) (dest + dest_pos)) + 2) = *(data + i + 3);
            *(((u8 *) (dest + dest_pos)) + 3) = *(data + i + 2);
        }
        /* handle any remaining bytes...
         * since byte_count is supposed to be multiple of 2,
         * (and has already been verified above 'if (data[2] != 2*count)')
         * this will be either 2, or none at all!
         */
        if (i + 1 < byte_count)
            *(((u8 *) (dest + dest_pos)) + 0) = *(data + i + 1);
        *(((u8 *) (dest + dest_pos)) + 1) = *(data + i + 0);
        *(((u8 *) (dest + dest_pos)) + 2) = 0;
        *(((u8 *) (dest + dest_pos)) + 3) = 0;
    }

    return response_length;
}

/* FUNCTION 0x03   - Read Holding Registers */
int read_output_words(u8 slave,
                      u16 start_addr,
                      u16 count,
                      u16 *dest,
                      int dest_size,
                      int send_retries,
                      u8 *error_code,
                      uint32_t response_timeout,
                      void *data_access_mutex,
                      u8 crc_reverse, client_node_t *node
) {
    if (count > MAX_READ_REGS) {
        count = MAX_READ_REGS;
#ifdef DEBUG
        logger.debug( "Too many registers requested.\n");
#endif
    }

    return read_registers(0x03 /* function */,
                          slave, start_addr, count, dest, dest_size, send_retries, error_code, response_timeout,
                          data_access_mutex, crc_reverse, node);
}

/* FUNCTION 0x03   - Read Holding Registers
 * u16 registers are stored in array of uint32_t, two registers per uint32_t.
 * Unused bits of last uint32_t element are set to 0.
 */
int read_output_words_u32(u8 slave,
                          u16 start_addr,
                          u16 count,
                          uint32_t *dest,
                          int send_retries,
                          u8 *error_code,
                          uint32_t response_timeout,
                          u8 crc_reverse, client_node_t *node) {
    if (count > MAX_READ_REGS) {
        count = MAX_READ_REGS;
#ifdef DEBUG
        logger.debug( "Too many registers requested.\n");
#endif
    }

    return read_registers_u32(0x03 /* function */,
                              slave, start_addr, count, dest, send_retries, error_code, response_timeout, crc_reverse,
                              node);
}

/* FUNCTION 0x03   - Read Holding Registers
 * return the array with the data to the calling function
 */
int read_output_words_u16_ref(u8 slave,
                              u16 start_addr,
                              u16 count,
                              u16 **dest,
                              int send_retries,
                              u8 *error_code,
                              uint32_t response_timeout,
                              u8 crc_reverse, client_node_t *node) {
    if (count > MAX_READ_REGS) {
        count = MAX_READ_REGS;
#ifdef DEBUG
        logger.debug( "Too many registers requested.\n");
#endif
    }

    return read_registers_u16_ref(0x03 /* function */,
                                  slave, start_addr, count, dest, send_retries, error_code, response_timeout,
                                  crc_reverse, node);
}

/* FUNCTION 0x04   - Read Input Registers */
int read_input_words(u8 slave,
                     u16 start_addr,
                     u16 count,
                     u16 *dest,
                     int dest_size,
                     int send_retries,
                     u8 *error_code,
                     uint32_t response_timeout,
                     void *data_access_mutex,
                     u8 crc_reverse, client_node_t *node
) {
    if (count > MAX_READ_REGS) {
        count = MAX_READ_REGS;
#ifdef DEBUG
        logger.debug( "Too many input registers requested.\n");
#endif
    }

    return read_registers(0x04 /* function */, slave, start_addr, count, dest, dest_size, send_retries, error_code,
                          response_timeout, data_access_mutex, crc_reverse, node);
}

/* FUNCTION 0x04   - Read Input Registers
 * u16 registers are stored in array of uint32_t, two registers per uint32_t.
 * Unused bits of last uint32_t element are set to 0.
 */
int read_input_words_u32(u8 slave,
                         u16 start_addr,
                         u16 count,
                         uint32_t *dest,
                         int send_retries,
                         u8 *error_code,
                         uint32_t response_timeout,
                         u8 crc_reverse, client_node_t *node) {
    if (count > MAX_READ_REGS) {
        count = MAX_READ_REGS;
#ifdef DEBUG
        logger.debug( "Too many input registers requested.\n");
#endif
    }

    return read_registers_u32(0x04 /* function */, slave, start_addr, count, dest, send_retries, error_code,
                              response_timeout, crc_reverse, node);
}

/* FUNCTION 0x04   - Read Input Registers
 * return the array with the data to the calling function
 */
int read_input_words_u16_ref(u8 slave,
                             u16 start_addr,
                             u16 count,
                             u16 **dest,
                             int send_retries,
                             u8 *error_code,
                             uint32_t response_timeout,
                             u8 crc_reverse, client_node_t *node) {
    if (count > MAX_READ_REGS) {
        count = MAX_READ_REGS;
#ifdef DEBUG
        logger.debug( "Too many input registers requested.\n");
#endif
    }

    return read_registers_u16_ref(0x04 /* function */, slave, start_addr, count, dest, send_retries, error_code,
                                  response_timeout, crc_reverse, node);
}

/* Execute a transaction for functions that WRITE a sinlge BIT.
 * Called by:  write_output_bit()
 *             write_output_word()
 */
static int set_single(u8 function,
                      u8 slave,
                      u16 addr,
                      u16 value,
                      int send_retries,
                      u8 *error_code,
                      uint32_t response_timeout,
                      void *data_access_mutex,
                      u8 crc_reverse, client_node_t *node
) {
    u8 packet[QUERY_BUFFER_SIZE];
    u8 *data;
    int query_length;
    int response_length;

    if (nullptr != data_access_mutex)
        Rtos::mutex_lock(data_access_mutex, -1);
    query_length = build_packet(slave, function, addr, value, packet);
    if (nullptr != data_access_mutex)
        Rtos::mutex_unlock(data_access_mutex);
    if (query_length < 0)
        return INTERNAL_ERROR;

    response_length = mb_transaction(packet, query_length, &data, send_retries, error_code, response_timeout,
                                     crc_reverse, node);

    if (response_length < 0)
        return response_length;
    if (response_length != 6)
        return INVALID_FRAME;

    if ((data[0] != slave) || (data[2] != packet[2]) || (data[3] != packet[3]) ||
        (data[4] != packet[4]) || (data[5] != packet[5]))
        return INVALID_FRAME;

    return response_length;
}

/* FUNCTION 0x05   - Force Single Coil */
int write_output_bit(u8 slave,
                     u16 coil_addr,
                     u16 state,
                     int send_retries,
                     u8 *error_code,
                     uint32_t response_timeout,
                     void *data_access_mutex,
                     u8 crc_reverse, client_node_t *node
) {
    if (state)
        state = 0xFF00;

    return set_single(0x05 /* function */, slave, coil_addr, state, send_retries, error_code, response_timeout,
                      data_access_mutex, crc_reverse, node);
}

/* FUNCTION 0x06   - Write Single Register */
int write_output_word(u8 slave,
                      u16 reg_addr,
                      u16 value,
                      int send_retries,
                      u8 *error_code,
                      uint32_t response_timeout,
                      void *data_access_mutex,
                      u8 crc_reverse, client_node_t *node
) {
    return set_single(0x06 /* function */,
                      slave, reg_addr, value, send_retries,
                      error_code, response_timeout, data_access_mutex, crc_reverse, node);
}

/* FUNCTION 0x0F   - Force Multiple Coils */
int write_output_bits(u8 slave,
                      u16 start_addr,
                      u16 coil_count,
                      u16 *data,
                      int send_retries,
                      u8 *error_code,
                      uint32_t response_timeout,
                      void *data_access_mutex,
                      u8 crc_reverse, client_node_t *node
) {
    int byte_count;
    u8 bit;
    int coil_check = 0;
    int data_array_pos = 0;
    int query_length;
    int response_length;
    u8 packet[QUERY_BUFFER_SIZE];
    u8 *rdata;

    if (coil_count > MAX_WRITE_COILS) {
        coil_count = MAX_WRITE_COILS;
#ifdef DEBUG
        logger.debug( "Writing to too many coils.\n");
#endif
    }

    query_length = build_packet(slave, 0x0F /* function */,
                                start_addr, coil_count, packet);
    if (query_length < 0)
        return INTERNAL_ERROR;

    /* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */
    byte_count = (u8) ((coil_count + 7) / 8);
    packet[query_length] = (u8) byte_count;

    if (nullptr != data_access_mutex)
        Rtos::mutex_lock(data_access_mutex);
    bit = 0x01;
    for (int i = 0; i < byte_count; i++) {
        packet[++query_length] = 0;
        while ((bit & 0xFF) && (coil_check < coil_count)) {
            coil_check++;
            if (data[data_array_pos++]) {
                packet[query_length] |= bit;
            } else {
                packet[query_length] &= ~bit;
            }
            bit <<= 1;
        }
        bit = 0x01;
    }
    if (nullptr != data_access_mutex)
        Rtos::mutex_unlock(data_access_mutex);
    response_length = mb_transaction(packet, ++query_length, &rdata, send_retries, error_code, response_timeout,
                                     crc_reverse, node);

    if (response_length < 0)
        return response_length;
    if (response_length != 6)
        return INVALID_FRAME;
    if ((rdata[2] != packet[2]) ||
        (rdata[3] != packet[3]) ||
        (rdata[4] != packet[4]) ||
        (rdata[5] != packet[5]))
        return INVALID_FRAME;

    return response_length;
}

/* FUNCTION 0x0F   - Force Multiple Coils
 * Bits should be stored on an uint32_t array, 32 bits per uint32_t.
 * Unused bits in last uint32_t should be set to 0.
 */
int write_output_bits_u32(u8 slave,
                          u16 start_addr,
                          u16 coil_count,
                          uint32_t *data,
                          int send_retries,
                          u8 *error_code,
                          uint32_t response_timeout,
                          u8 crc_reverse, client_node_t *node) {
    int org_pos;
    int byte_count;
    int i;
    int response_length;
    int query_length;
    u8 packet[QUERY_BUFFER_SIZE];
    u8 *rdata;

    if (coil_count > MAX_WRITE_COILS) {
        coil_count = MAX_WRITE_COILS;
#ifdef DEBUG
        logger.debug( "Writing to too many coils.\n");
#endif
    }

    query_length = build_packet(slave, 0x0F /* function */,
                                start_addr, coil_count, packet);
    if (query_length < 0)
        return INTERNAL_ERROR;

    /* NOTE: Integer division. This is equivalent of determining the ceil(count/8) */
    byte_count = (coil_count + 7) / 8;
    packet[query_length] = (u8) byte_count;

    /* handle groups of 4 bytes... */
    for (i = 0, org_pos = 0; i + 3 < byte_count; i += 4, org_pos++) {
        packet[++query_length] = data[org_pos] & 0xFF;
        data[org_pos] >>= 8;
        packet[++query_length] = data[org_pos] & 0xFF;
        data[org_pos] >>= 8;
        packet[++query_length] = data[org_pos] & 0xFF;
        data[org_pos] >>= 8;
        packet[++query_length] = data[org_pos] & 0xFF;
    }
    /* handle any remaining bytes... */
    for (; i < byte_count; i++) {
        packet[++query_length] = data[org_pos] & 0xFF;
        data[org_pos] >>= 8;
    }

    response_length = mb_transaction(packet, ++query_length, &rdata, send_retries, error_code, response_timeout,
                                     crc_reverse, node);

    if (response_length < 0)
        return response_length;
    if (response_length != 6)
        return INVALID_FRAME;
    if ((rdata[2] != packet[2]) ||
        (rdata[3] != packet[3]) ||
        (rdata[4] != packet[4]) ||
        (rdata[5] != packet[5]))
        return INVALID_FRAME;

    return response_length;
}

/* FUNCTION 0x10   - Force Multiple Registers */
int write_output_words(u8 slave,
                       u16 start_addr,
                       u16 reg_count,
                       u16 *data,
                       int send_retries,
                       u8 *error_code,
                       uint32_t response_timeout,
                       void *data_access_mutex,
                       u8 crc_reverse, client_node_t *node
) {
    u8 byte_count;
    int query_length;
    int response_length;
    u8 packet[QUERY_BUFFER_SIZE];
    u8 *rdata;

    if (reg_count > MAX_WRITE_REGS) {
        reg_count = MAX_WRITE_REGS;
#ifdef DEBUG
        logger.debug( "Trying to write to too many registers.\n");
#endif
    }

    query_length = build_packet(slave, 0x10 /* function */,
                                start_addr, reg_count, packet);
    if (query_length < 0)
        return INTERNAL_ERROR;

    byte_count = (u8) (reg_count * 2);
    packet[query_length] = byte_count;

    if (nullptr != data_access_mutex)
        Rtos::mutex_lock(data_access_mutex);
    for (int i = 0; i < reg_count; i++) {
        packet[++query_length] = data[i] >> 8;
        packet[++query_length] = data[i] & 0x00FF;
    }
    if (nullptr != data_access_mutex)
        Rtos::mutex_unlock(data_access_mutex);

    response_length = mb_transaction(packet, ++query_length, &rdata, send_retries, error_code, response_timeout,
                                     crc_reverse, node);

    if (response_length < 0)
        return response_length;
    if (response_length != 6)
        return INVALID_FRAME;
    if ((rdata[2] != packet[2]) ||
        (rdata[3] != packet[3]) ||
        (rdata[4] != packet[4]) ||
        (rdata[5] != packet[5]))
        return INVALID_FRAME;

    return response_length;
}

/* FUNCTION 0x10   - Force Multiple Registers
 * u16 registers are stored in array of uint32_t, two registers per uint32_t.
 * Unused bits of last uint32_t element are set to 0.
 */
int write_output_words_u32(u8 slave,
                           u16 start_addr,
        /* number of 16 bit registers packed in the uint32_t array! */
                           u16 reg_count,
                           uint32_t *data,
                           int send_retries,
                           u8 *error_code,
                           uint32_t response_timeout,
                           u8 crc_reverse, client_node_t *node) {
    u8 byte_count;
    int i;
    int query_length;
    int response_length;
    u8 packet_[QUERY_BUFFER_SIZE];
    u8 *packet = packet_; /* remove the const'ness of packet_ */
    u8 *rdata;

    if (reg_count > MAX_WRITE_REGS) {
        reg_count = MAX_WRITE_REGS;
#ifdef DEBUG
        logger.debug( "Trying to write to too many registers.\n");
#endif
    }

    /* Make sure that the de-referencing and up-casting going on later on in
     * this function, i.e. code like the following line:
     * *((u16 *)packet) = XXX
     * will result in u16 words starting off on even addresses.
     * If we don't do this, some compilers (e.g. AVR32 cross-compiler) will
     * generate code which, when executed, will result in 'bus error'.
     *
     * The following packet++ means that the first byte of the packet array is
     * essentially never used. Notice too that the size of thepacket array
     * already takes into account this un-used byte.
     */
    packet++;

    query_length = build_packet(slave, 0x10 /* function */,
                                start_addr, reg_count, packet);
    if (query_length < 0)
        return INTERNAL_ERROR;

    byte_count = (u8) (reg_count * 2);
    packet[query_length] = byte_count;

    /* handle groups of 4 bytes... */
    for (i = 0; 4 * i + 3 < byte_count; i++) {
        *((u16 *) (packet + (++query_length))) = mb_hton((u16) data[i]);
        ++query_length;
        *((u16 *) (packet + (++query_length))) = mb_hton(data[i] >> 16);
        ++query_length;
    }

    /* handle any remaining bytes...
     * since byte_count is supposed to be multiple of 2,
     * (and has already been verified above 'if (data[2] != 2*count)')
     * this will be either 2, or none at all!
     */
    if (4 * i + 1 < byte_count) {
        *((u16 *) (packet + (++query_length))) = mb_hton((u16) data[i]);
        ++query_length;
    }

    response_length = mb_transaction(packet, ++query_length, &rdata, send_retries, error_code, response_timeout,
                                     crc_reverse, node);

    if (response_length < 0)
        return response_length;
    if (response_length != 6)
        return INVALID_FRAME;
    if ((rdata[2] != packet[2]) ||
        (rdata[3] != packet[3]) ||
        (rdata[4] != packet[4]) ||
        (rdata[5] != packet[5]))
        return INVALID_FRAME;

    return response_length;
}

/************************************************/
/************************************************/
/**                                            **/
/**   Modbus Library Management Functions.     **/
/**                                            **/
/************************************************/
/************************************************/

/* Initialise the Modbus Master Layer */
int mb_master_init__(int extra_bytes) {
#ifdef DEBUG
    logger.debug( "mb_master_init__(extra_bytes=%d), QUERY_BUFFER_SIZE=%d\n", extra_bytes, QUERY_BUFFER_SIZE);
#endif
    buff_extra_bytes_ = extra_bytes;
    return 0;
}

/* Shut down the Modbus Master Layer */
int mb_master_done__() {
    return 0;
}

#if 0
int mb_master_init(int nd_count) {
  int extra_bytes;

#ifdef DEBUG
  fprintf( stderr, "mb_master_init()\n");
  fprintf( stderr, "creating %d nodes\n", nd_count);
#endif

  /* initialise layer 1 library */
  if (modbus_init(nd_count, DEF_OPTIMIZATION, &extra_bytes) < 0)
    goto error_exit_0;

  /* initialise this library */
  if (mb_master_init__(extra_bytes) < 0)
    goto error_exit_1;

  return 0;

error_exit_1:
  modbus_done();
error_exit_0:
  return -1;
}


int mb_master_done(void) {
  mb_master_done__();
  return modbus_done();
}
#endif

/* Establish a connection to a remote server/slave */
/* NOTE: We use the lower 2 bits of the returned node id to identify which
 *       layer1 implementation to use.
 *           0 -> TCP
 *           1 -> RTU
 *           2 -> ASCII
 *           4 -> unused
 *       The node id used by the layer1 is shifted left 2 bits
 *       before returning the node id to the caller!
 */
int mb_master_connect(client_node_t *node) {
    int res = -1;

#ifdef DEBUG
    logger.debug( "mb_master_tcp connect()\n");
#endif

    /* call layer 1 library */
    switch (node->node_address.naf) {
#ifdef MODBUS_TCP
        case naf_tcp:
            node->nd = modbus_tcp_connect(&node->node_address);
          if (res >= 0)
            res = res * 4 + 0 /* offset into fptr_ with TCP functions */;
          return res;
#endif
#ifdef MODBUS_RTU
        case naf_rtu:
            node->nd = modbus_rtu_connect(&node->node_address);
            if (res >= 0)
                res = res * 4 + 1 /* offset into fptr_ with RTU functions */;
            return res;
#endif
#ifdef MODBUS_ASCII
            case naf_ascii:
              res = modbus_ascii_connect(node_addr);
              if (res >= 0)
                res = res * 4 + 2 /* offset into fptr_ with ASCII functions */;
              return res;
#endif
        default:
            logger.error("naf:%d", node->node_address.naf);
    }

    return -1;
}

/* Shut down a connection to a remote server/slave */
int mb_master_close(client_node_t *node) {
#ifdef DEBUG
    logger.debug( "mb_master_close(): nd = %d\n", node->fd);
#endif
    get_ttyfd() /* declare the ttyfd variable, ... */
    /* call layer 1 library */
    return modbus_close(ttyfd);
}

/* Tell the library that communications will be suspended for some time. */
/* RTU and ASCII versions ignore this function
 * TCP version closes all the open tcp connections (connections are automatically
 *   re-established the next time an IO function to the slave is requested).
 *   To be more precise, the TCP version makes an estimate of how long
 *   the silence will be based on previous invocations to this exact same
 *   function, and will only close the connections if this silence is
 *   expected to be longer than 1 second!
 *   (The closing of connections is specified in Modbus specification)
 */
int mb_master_tcp_silence_init() {
#ifdef DEBUG
    logger.debug( "mb_master_silence_init():\n");
#endif
    /* call layer 1 library */
    return modbus_tcp_silence_init();
}
#endif
